﻿using System;

namespace RayDen.Library.Core.Primitives {
    public class Transform {
        internal Matrix4x4 m, mInv;
        public static Matrix4x4 MAT_IDENTITY = new Matrix4x4().Identity();
        public Transform() {
            m = mInv = MAT_IDENTITY;
        }

        public Transform(float[,] mat) {
            Matrix4x4 o = new Matrix4x4(mat[0, 0], mat[0, 1], mat[0, 2], mat[0, 3],
                                        mat[1, 0], mat[1, 1], mat[1, 2], mat[1, 3],
                                        mat[2, 0], mat[2, 1], mat[2, 2], mat[2, 3],
                                        mat[3, 0], mat[3, 1], mat[3, 2], mat[3, 3]);
            m = o;
            mInv = Matrix4x4.Inverse(ref m);
        }

        public Transform(ref Matrix4x4 mat) {
            m = mat;
            mInv = Matrix4x4.Inverse(ref mat);
        }

        public Transform(ref Matrix4x4 mat,
                         ref Matrix4x4 minv) {
            m = mat;
            mInv = minv;
        }

        public Transform(Matrix4x4 mat,
                         Matrix4x4 minv) {
            m = mat;
            mInv = minv;
        }


        public Transform GetInverse() {
            return new Transform(ref mInv, ref m);
        }

        public Matrix4x4 GetMatrix() {
            return m;
        }

        public static Point operator &(Transform tr, Point pt) {
            float x = pt.x, y = pt.y, z = pt.z;
            float xp = tr.m.m00 * x + tr.m.m01 * y + tr.m.m02 * z + tr.m.m03;
            float yp = tr.m.m10 * x + tr.m.m11 * y + tr.m.m12 * z + tr.m.m13;
            float zp = tr.m.m20 * x + tr.m.m21 * y + tr.m.m22 * z + tr.m.m23;
            float wp = tr.m.m30 * x + tr.m.m31 * y + tr.m.m32 * z + tr.m.m33;
            if (wp.NearEqual(1f)) return new Point(xp, yp, zp);
            else return (new Point(xp, yp, zp) / wp);
        }
        /*
        public static Ray operator &(Transform tr, Ray r) {
            return new Ray(tr & r.Org, tr & r.Dir, r.minT, r.maxT);
        }
        */
        public static Vector operator &(Transform tr, Vector pt) {
            float x = pt.x, y = pt.y, z = pt.z;
            float xp = tr.m.m00 * x + tr.m.m01 * y + tr.m.m02 * z;
            float yp = tr.m.m10 * x + tr.m.m11 * y + tr.m.m12 * z;
            float zp = tr.m.m20 * x + tr.m.m21 * y + tr.m.m22 * z;
            return new Vector(xp, yp, zp);
        }

        public static Normal operator &(Transform tr, Normal pt) {
            float x = pt.x, y = pt.y, z = pt.z;
            float xp = tr.mInv.m00 * x + tr.mInv.m01 * y + tr.mInv.m02 * z;
            float yp = tr.mInv.m10 * x + tr.mInv.m11 * y + tr.mInv.m12 * z;
            float zp = tr.mInv.m20 * x + tr.mInv.m21 * y + tr.mInv.m22 * z;
            return new Normal(xp, yp, zp);
        }


        public bool SwapsHandedness() {
            float det = ((m.m00 *
                          (m.m11 * m.m22 -
                           m.m12 * m.m21)) -
                         (m.m01 *
                          (m.m10 * m.m22 -
                           m.m12 * m.m20)) +
                         (m.m02 *
                          (m.m10 * m.m21 -
                           m.m11 * m.m20)));
            return det < 0f;
        }

        public static Transform Translate(Vector delta) {
            Matrix4x4 m = new Matrix4x4(1, 0, 0, delta.x,
                                        0, 1, 0, delta.y,
                                        0, 0, 1, delta.z,
                                        0, 0, 0, 1);
            Matrix4x4 minv = new Matrix4x4(1, 0, 0, -delta.x,
                                           0, 1, 0, -delta.y,
                                           0, 0, 1, -delta.z,
                                           0, 0, 0, 1);
            return new Transform(ref m, ref minv);
        }

        public static Transform Scale(float x, float y, float z) {
            Matrix4x4 m = new Matrix4x4(x, 0, 0, 0,
                                        0, y, 0, 0,
                                        0, 0, z, 0,
                                        0, 0, 0, 1);
            Matrix4x4 minv = new Matrix4x4(1f / x, 0, 0, 0,
                                           0, 1f / y, 0, 0,
                                           0, 0, 1f / z, 0,
                                           0, 0, 0, 1);
            return new Transform(ref m, ref minv);
        }

        public static Transform RotateX(float angle) {
            float sin_t = (float)Math.Sin(MathLab.Radians(angle));
            float cos_t = (float)Math.Cos(MathLab.Radians(angle));
            Matrix4x4 m = new Matrix4x4(1, 0, 0, 0,
                                        0, cos_t, -sin_t, 0,
                                        0, sin_t, cos_t, 0,
                                        0, 0, 0, 1);
            var mt = m.Transpose();
            return new Transform(ref m, ref mt);
        }

        public static Transform RotateY(float angle) {
            float sin_t = (float)Math.Sin(MathLab.Radians(angle));
            float cos_t = (float)Math.Cos(MathLab.Radians(angle));
            Matrix4x4 m = new Matrix4x4(cos_t, 0, sin_t, 0,
                                        0, 1, 0, 0,
                                        -sin_t, 0, cos_t, 0,
                                        0, 0, 0, 1);
            return new Transform(m, m.Transpose());
        }

        public static Transform RotateZ(float angle) {
            float sin_t = (float)Math.Sin(MathLab.Radians(angle));
            float cos_t = (float)Math.Cos(MathLab.Radians(angle));
            Matrix4x4 m = new Matrix4x4(cos_t, -sin_t, 0, 0,
                                        sin_t, cos_t, 0, 0,
                                        0, 0, 1, 0,
                                        0, 0, 0, 1);
            return new Transform(m, m.Transpose());
        }

        public static Transform Rotate(float angle, Vector axis) {
            Vector a = axis.Normalize();
            float s = (float)Math.Sin(MathLab.Radians(angle));
            float c = (float)Math.Cos(MathLab.Radians(angle));
            float[,] m = new float[4, 4];

            m[0, 0] = a.x * a.x + (1f - a.x * a.x) * c;
            m[0, 1] = a.x * a.y * (1f - c) - a.z * s;
            m[0, 2] = a.x * a.z * (1f - c) + a.y * s;
            m[0, 3] = 0;

            m[1, 0] = a.x * a.y * (1f - c) + a.z * s;
            m[1, 1] = a.y * a.y + (1f - a.y * a.y) * c;
            m[1, 2] = a.y * a.z * (1f - c) - a.x * s;
            m[1, 3] = 0;

            m[2, 0] = a.x * a.z * (1f - c) - a.y * s;
            m[2, 1] = a.y * a.z * (1f - c) + a.x * s;
            m[2, 2] = a.z * a.z + (1f - a.z * a.z) * c;
            m[2, 3] = 0;

            m[3, 0] = 0;
            m[3, 1] = 0;
            m[3, 2] = 0;
            m[3, 3] = 1;

            Matrix4x4 o = new Matrix4x4(m);
            return new Transform(o, o.Transpose());
        }

        public static Transform LookAt(Point pos, Point look, Vector up)
        {
            float[,] m = new float[4, 4];

            // Initialize fourth column of viewing matrix
            m[0, 3] = pos.x;
            m[1, 3] = pos.y;
            m[2, 3] = pos.z;
            m[3, 3] = 1;
            Vector dir = (look - pos);
            dir.Normalize();
            Vector right = (dir ^ up).Normalize();
            Vector newUp = right ^ dir;
            m[0, 0] = right.x;
            m[1, 0] = right.y;
            m[2, 0] = right.z;
            m[3, 0] = 0f;
            m[0, 1] = newUp.x;
            m[1, 1] = newUp.y;
            m[2, 1] = newUp.z;
            m[3, 1] = 0f;
            m[0, 2] = dir.x;
            m[1, 2] = dir.y;
            m[2, 2] = dir.z;
            m[3, 2] = 0f;
            Matrix4x4 camToWorld = new Matrix4x4(m);
            return new Transform(Matrix4x4.Inverse(ref camToWorld), camToWorld);
        }




        public static AABB operator &(Transform tr, AABB b) {
            Transform M = tr;
            Point pt = new Point(b.Min.x, b.Min.y, b.Min.z);
            AABB ret = new AABB((tr & pt));


            ret.Union(tr & new Point(b.Max.x, b.Min.y, b.Min.z));
            ret.Union(tr & new Point(b.Min.x, b.Max.y, b.Min.z));
            ret.Union(tr & new Point(b.Min.x, b.Min.y, b.Max.z));
            ret.Union(tr & new Point(b.Min.x, b.Max.y, b.Max.z));
            ret.Union(tr & new Point(b.Max.x, b.Max.y, b.Min.z));
            ret.Union(tr & new Point(b.Max.x, b.Min.y, b.Max.z));
            ret.Union(tr & new Point(b.Max.x, b.Max.y, b.Max.z));
            return ret;
        }

        public static Transform operator *(Transform t1, Transform t2) {
            Matrix4x4 m1 = t1.m * t2.m;
            Matrix4x4 m2 = t2.mInv * t1.mInv;
            return new Transform(m1, m2);
        }

        public static void TransformAccordingNormal(Normal nn, Vector woL, ref Vector woW) {
            Vector sn = new Vector(), tn;
            float zz = (float)Math.Sqrt(1f - nn.z * nn.z);
            sn.z = 0f;
            if (Math.Abs(zz) < 1e-6f) {
                sn.x = 1f;
                sn.y = 0f;
            }
            else {
                sn.x = nn.y / zz;
                sn.y = -nn.x / zz;
            }
            tn = (nn.ToVec() ^ sn);
            woW = woL.x * sn + woL.y * tn + woL.z * (new Vector(nn.x, nn.y, nn.z));
        }

        public static Transform Orthographic(float znear, float zfar) {
            return Scale(1f, 1f, 1f / (zfar - znear)) *
                   Translate(new Vector(0f, 0f, -znear));
        }

        public static Transform Perspective(float fov, float n, float f) {
            // Perform projective divide
            float inv_denom = 1f / (1f - n / f);
            Matrix4x4 persp = new Matrix4x4(1, 0, 0, 0,
                                            0, 1, 0, 0,
                                            0, 0, inv_denom, -n * inv_denom,
                                            0, 0, 1, 0);
            // Scale to canonical viewing volume
            float invTanAng = 1f / (float)Math.Tan(MathLab.Radians(fov) / 2f);
            return Scale(invTanAng, invTanAng, 1) * (new Transform(ref persp));
        }
        public bool HasScale() {
            float det = Math.Abs(m.m00 * (m.m11 * m.m22 - m.m12 * m.m21)) -
                        (m.m01 * (m.m10 * m.m22 - m.m12 * m.m20)) +
                        (m.m02 * (m.m10 * m.m21 - m.m11 * m.m20));
            return (det < .999f || det > 1.001f);
        }
    }
}